/**
 * @file main.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 华为2023软件挑战赛程序入口
 */

#include <cstdio>
#include <cstring>
#include "log.h"
#include "utils.h"
#include "Params.h"
#include "Map.h"
#include "Bench.h"
#include "Robot.h"
#include "Dispatcher.h"
#include "AStar.h"
#include "BFS.h"
#include "Path.h"
#include "Planner.h"

Bench benchs[BENCH_NUM_MAX]; // 工作台
Robot robots[ROBOT_NUM_MAX]; // 机器人
Dispatcher dispatchers[ROBOT_NUM_MAX]; // 机器人调度器

int main(int argc, char *argv[]) {
    /* 程序初始化 */////////////////////////////////////////////////////////////////////////////////////////////////
    #ifdef USE_LOG
    LOG_INIT();
    #endif

    /* 地图初始化 */////////////////////////////////////////////////////////////////////////////////////////////////
    char buff[128]; // 输入缓存
    int bCnt = 0; // 工作台数量
    int rCnt = 0; // 机器人数量
    for(int y = MAP_Y_MAX - 1; std::fgets(buff, sizeof buff, stdin); --y) {
        if (buff[0] == 'O' && buff[1] == 'K') {
            break;
        }
        for(int x = 0; x < MAP_X_MAX ; ++x) {
            Map::setPoint(x, y, buff[x]);
            if(buff[x] >= '1' && buff[x] <= '9') {
                benchs[bCnt].init(bCnt, buff[x], x, y);
                ++bCnt;
            } else if(buff[x] == 'A') {
                robots[rCnt].init(rCnt, x, y);
                dispatchers[rCnt].attach(&robots[rCnt]);
                ++rCnt;
            }
        }
    }
    Bench::setTotalNumber(bCnt);
    switch(Bench::getTotalNumber()) {
        case 9:
            Map::setID(1);
            break;

        case 27:
            Map::setID(2);
            break;

        default:
            Map::setID(-1);
            break;
    }
    if(Bench::getTotalNumber() == 9) {
        Map::setID(1);
    } else if(Bench::getTotalNumber() == 27) {
        Map::setID(2);
    } else if(Bench::getTotalNumber() <= 10) {
        Map::setID(3);
    } else {
        Map::setID(4);
    }
    PARAMS.setMap(Map::getID());
    const int BENCH_NUM = Bench::getTotalNumber(); // 工作台总数
    const int ROBOT_NUM = ROBOT_NUM_MAX; // 机器人总数
    Map::init();

    #ifdef USE_LOG
    // Map::printToLog();
    #endif

    for(int id = 0; id < ROBOT_NUM; ++id) {
        robots[id].initPID(PARAMS.FORWARD_PID_EMPTY, PARAMS.FORWARD_PID_FULL,
                           PARAMS.ROTATE_PID_EMPTY, PARAMS.ROTATE_PID_FULL);
        robots[id].forwardPID.setPrecision(PARAMS.FORWARD_FINISH_PRECISION);
        robots[id].rotatePID.setPrecision(PARAMS.ROTATE_FINISH_PRECISION);
    }
    Map::refreshBenchLocation(benchs);
    Map::refreshRobotLocation(dispatchers);
    Path::init();
    Planner::loop(dispatchers, benchs, FRAME_NUM_MAX);

    /* 响应判题器 */////////////////////////////////////////////////////////////////////////////////////////////////
    std::puts("OK");
    std::fflush(stdout);

    /* 帧交互 *////////////////////////////////////////////////////////////////////////////////////////////////////
    int frameID; // 帧序号
    int money; // 当前金钱数
    #ifdef USE_LOG
    int lastFrameID = 0; // 上次循环的帧序号
    int initTimeoutFrames = 0; // 初始化未完成导致的跳帧次数
    int jumpFramesSum = 0; // 跳帧次数和
    #endif
    while(std::scanf("%d %d\n", &frameID, &money) != EOF) {
        /* 跳帧检测 *///===========================================================================================
        #ifdef USE_LOG
        int jumpFrames = frameID - lastFrameID - 1;
        if(lastFrameID == 0) {
            initTimeoutFrames = jumpFrames;
        } else {
            jumpFramesSum += jumpFrames;
        }
        lastFrameID = frameID;
        #endif
        /* 读取工作台信息 *///======================================================================================
        int benchNum;
        std::scanf("%d\n", &benchNum);
        for(int id = 0; id < benchNum; id++) {
            int type, remainFrames, materialStatus, productStatus;
            double coord_x, coord_y;
            std::scanf("%d %lf %lf %d %d %d\n",
                       &type, &coord_x, &coord_y, &remainFrames, &materialStatus, &productStatus);
            benchs[id].refresh(type, coord_x, coord_y, remainFrames, materialStatus, productStatus);
        }

        /* 读取机器人信息 *///======================================================================================
        for(int id = 0; id < ROBOT_NUM_MAX; id++) {
            int benchID, materialType;
            double timeFactor, crashFactor, angleSpeed, lineSpeed_x, lineSpeed_y, yaw, coord_x, coord_y;
            std::scanf("%d %d %lf %lf %lf %lf %lf %lf %lf %lf\n", &benchID, &materialType, &timeFactor,
                       &crashFactor, &angleSpeed, &lineSpeed_x, &lineSpeed_y, &yaw, &coord_x, &coord_y);
            robots[id].refresh(benchID, materialType, timeFactor, crashFactor, angleSpeed,
                               lineSpeed_x, lineSpeed_y, yaw, coord_x, coord_y);
        }
        Map::refreshRobotLocation(dispatchers);

        /* 读OK *///=============================================================================================
        char buff[128]; // 输入缓存
        while (std::fgets(buff, sizeof buff, stdin)) {
            if (buff[0] == 'O' && buff[1] == 'K') {
                break;
            } else {
                LOG("[ERROR] Bad input, remain: %s", buff);
                return -1;
            }
        }

        /* 输出帧ID *///==========================================================================================
        std::printf("%d\n", frameID);

        /* 方案规划 *///===========================================================================================
        Planner::loop(dispatchers, benchs, FRAME_NUM_MAX - frameID);

        /* 冲突检测 *///===========================================================================================
        if(Map::getID() != 1) {
            int crash[ROBOT_NUM_MAX];
            for(int id = 0; id < ROBOT_NUM; ++id) {
                crash[id] = dispatchers[id].forecastCrash(PARAMS.FORCAST_STEPS, true);
            }
            for(int id = 0; id < ROBOT_NUM; ++id) {
                if(crash[id] != -1 && crash[crash[id]] == id) {
                    if(!dispatchers[id].isAvoid(&dispatchers[crash[id]]) && !dispatchers[crash[id]].isAvoid(&dispatchers[id])) { // 如果碰撞没有被处理
                        int avoid = id; // 默认指定己方为避让方
                        if(dispatchers[id].isAvoid() + dispatchers[crash[id]].isAvoid() == 1) { // 冲突双方有一方正在避让别的机器人
                            avoid = dispatchers[id].isAvoid() ? id : crash[id]; // 处在避让态的一方避让
                        } else if(robots[id].isEmpty() + robots[crash[id]].isEmpty() == 1) { // 冲突双方有一方是空载
                            avoid = robots[id].isEmpty() ? id : crash[id]; // 空载的一方避让
                        }
                        if(!dispatchers[avoid].pushCmd_Avoid(&dispatchers[crash[avoid]], PARAMS.FORCAST_STEPS)) {
                            if(!dispatchers[crash[avoid]].pushCmd_Avoid(&dispatchers[avoid], PARAMS.FORCAST_STEPS)) {
                                // LOG("ERROR!\n");
                                // dispatchers[avoid].clearCmd();
                                // dispatchers[crash[avoid]].clearCmd();
                            }
                        }
                    }
                    crash[id] = crash[crash[id]] = -1; // 解除冲突
                }
            }
            for(int id = 0; id < ROBOT_NUM; ++id) {
                if(crash[id] != -1 && dispatchers[crash[id]].isFree()) { // 如果另一方处于空闲状态，令其主动避让
                    dispatchers[crash[id]].pushCmd_Avoid(&dispatchers[id], PARAMS.FORCAST_STEPS);
                }
            }
            for(int id = 0; id < ROBOT_NUM; ++id) {
                int obstacle = dispatchers[id].forecastCrash(PARAMS.SAFETY_STEPS, true);
                if(obstacle != -1) {
                    if(dispatchers[id].isAvoid(&dispatchers[obstacle])) {
                        continue;
                    } else if(!dispatchers[obstacle].isAvoid() && !(dispatchers[obstacle].isWait())) {
                        dispatchers[id].pushCmd_Wait();
                    }
                }
            }

        /* 机器人调度 *///=========================================================================================
            for(int id = 0; id < ROBOT_NUM; id++) {
                dispatchers[id].loop();
            }

        /* 运动控制 *///===========================================================================================
            for(int id = 0; id < ROBOT_NUM; id++) {
                robots[id].sendCtrlCommand();
            }
        } else {
            dispatchers[3].loop();
            robots[3].sendCtrlCommand();
        }

        /* 响应判题器 *///=========================================================================================
        std::printf("OK\n");
        std::fflush(stdout);
    }

    /* LOG打印 *///////////////////////////////////////////////////////////////////////////////////////////////////
    LOG("initTimeoutFrames[%d] jumpFramesSum[%d]\n", initTimeoutFrames, jumpFramesSum);

    return 0;
}
